// 包含API的头文件
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include "tf2/LinearMath/Quaternion.h"
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
 geometry_msgs::Pose pose;
 float x_ang= 1.57, y_ang = 0,  z_ang = 0;
float chg =0;
int main(int argc, char **argv)
{
    ros::init(argc, argv, "add_collision_objct");
    ros::NodeHandle nh;
    ros::AsyncSpinner spin(1);
    spin.start();

    static const std::string PLANNING_GROUP = "arm";



    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
    visual_tools.deleteAllMarkers();
    visual_tools.loadRemoteControl();
    //
      moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);


    const robot_state::JointModelGroup* joint_model_group =
    move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
    // 创建运动规划的情景，等待创建完成
    moveit::planning_interface::PlanningSceneInterface current_scene;
    sleep(1.0);

    std::vector<std::string> object_ids;
    object_ids.push_back("arm_cylinder");
    current_scene.removeCollisionObjects(object_ids);

    // 声明一个障碍物的实例，并且为其设置一个id，方便对其进行操作，该实例会发布到当前的情景实例中
    moveit_msgs::CollisionObject cylinder;
    cylinder.header.frame_id =  move_group.getPlanningFrame();
    cylinder.id = "arm_cylinder";

    // 设置障碍物的外形、尺寸等属性   
    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 0.02;
    primitive.dimensions[1] = 0.1;
    primitive.dimensions[2] = 0.1;

    // 设置障碍物的位置
    tf2::Quaternion qtn;
    qtn.setRPY(1.57,2.0,0); 
    pose.orientation.w = 0.0;
    pose.orientation.x = 0;
    pose.orientation.y = 0.0;
    pose.orientation.z = 0;


    pose.position.x =  0.0;
    pose.position.y =  0.1;
    pose.position.z =  0.5;

    // 将障碍物的属性、位置加入到障碍物的实例中
    cylinder.primitives.push_back(primitive);
    cylinder.primitive_poses.push_back(pose);
    cylinder.operation = cylinder.ADD;

    // 创建一个障碍物的列表，把之前创建的障碍物实例加入其中
    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(cylinder);

    // 所有障碍物加入列表后（这里只有一个障碍物），再把障碍物加入到当前的情景中，如果要删除障碍物，使用removeCollisionObjects(collision_objects)
    current_scene.addCollisionObjects(collision_objects);

    geometry_msgs::Pose target_pose1;
    target_pose1.orientation.w = 0.726282;
    target_pose1.orientation.x= 0;
    target_pose1.orientation.y = -0.687396;
    target_pose1.orientation.z = 0;

    target_pose1.position.x = 0.0;
    target_pose1.position.y = 0.2;
    target_pose1.position.z = 0.5;
    move_group.setPoseTarget(target_pose1);

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    moveit::planning_interface::MoveItErrorCode success = move_group.plan(my_plan);

    ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");   

    ROS_INFO_NAMED("trajectory_show", " the plan is %s",success?"succeed":"failed");

    visual_tools.publishAxisLabeled(target_pose1, "pose1");
    if(visual_tools.publishTrajectoryLine(my_plan.trajectory_,  joint_model_group))
    ROS_INFO("Vision ok");
    visual_tools.trigger();
    visual_tools.prompt("show trajectory");

    //让机械臂按照规划的轨迹开ss始运动。
    if(success)
     move_group.execute(my_plan);
    ros::shutdown(); 

    return 0;
}


